#include "Math3D.h"
#include "Vector2D.h"
#include "Vector3D.h"
#include "Vector4D.h"
#include "Quaternion.h"
#include "Ray.h"
#include "Sphere.h"
#include "AxisAlignedBox.h"
#include "Plane.h"

#include <algorithm>


const float Math::POS_INFINITY = std::numeric_limits<float>::infinity();
const float Math::NEG_INFINITY = -std::numeric_limits<float>::infinity();
const float Math::PI = float( 4.0 * atan( 1.0 ) );
const float Math::TWO_PI = float( 2.0 * PI );
const float Math::HALF_PI = float( 0.5 * PI );
const float Math::fDeg2Rad = PI / float(180.0);
const float Math::fRad2Deg = float(180.0) / PI;

int Math::mTrigTableSize;
Math::AngleUnit Math::msAngleUnit;

float  Math::mTrigTableFactor;
float *Math::mSinTable = NULL;
float *Math::mTanTable = NULL;

//-----------------------------------------------------------------------
Math::Math( unsigned int trigTableSize )
{
	msAngleUnit = AU_DEGREE;

	mTrigTableSize = trigTableSize;
	mTrigTableFactor = mTrigTableSize / Math::TWO_PI;

	mSinTable = new float[mTrigTableSize];
	mTanTable = new float[mTrigTableSize];

	buildTrigTables();
}

//-----------------------------------------------------------------------
Math::~Math()
{
	SAFE_DELETE_ARRAY(mSinTable);
	SAFE_DELETE_ARRAY(mTanTable);
}

//-----------------------------------------------------------------------
void Math::buildTrigTables(void)
{
	// Build trig lookup tables
	// Could get away with building only PI sized Sin table but simpler this
	// way. Who cares, it'll ony use an extra 8k of memory anyway and I like
	// simplicity.
	float angle;
	for (int i = 0; i < mTrigTableSize; ++i)
	{
		angle = Math::TWO_PI * i / mTrigTableSize;
		mSinTable[i] = sin(angle);
		mTanTable[i] = tan(angle);
	}
}
//-----------------------------------------------------------------------
float Math::SinTable (float fValue)
{
	// Convert range to index values, wrap if required
	int idx;
	if (fValue >= 0)
	{
		idx = int(fValue * mTrigTableFactor) % mTrigTableSize;
	}
	else
	{
		idx = mTrigTableSize - (int(-fValue * mTrigTableFactor) % mTrigTableSize) - 1;
	}

	return mSinTable[idx];
}
//-----------------------------------------------------------------------
float Math::TanTable (float fValue)
{
	// Convert range to index values, wrap if required
	int idx = int(fValue *= mTrigTableFactor) % mTrigTableSize;
	return mTanTable[idx];
}
//-----------------------------------------------------------------------
int Math::ISign (int iValue)
{
	return ( iValue > 0 ? +1 : ( iValue < 0 ? -1 : 0 ) );
}
//-----------------------------------------------------------------------
Radian Math::ACos (float fValue)
{
	if ( -1.0 < fValue )
	{
		if ( fValue < 1.0 )
			return Radian(acos(fValue));
		else
			return Radian(0.0);
	}
	else
	{
		return Radian(PI);
	}
}
//-----------------------------------------------------------------------
Radian Math::ASin (float fValue)
{
	if ( -1.0 < fValue )
	{
		if ( fValue < 1.0 )
			return Radian(asin(fValue));
		else
			return Radian(HALF_PI);
	}
	else
	{
		return Radian(-HALF_PI);
	}
}
//-----------------------------------------------------------------------
float Math::Sign (float fValue)
{
	if ( fValue > 0.0 )
		return 1.0;

	if ( fValue < 0.0 )
		return -1.0;

	return 0.0;
}
//-----------------------------------------------------------------------
float Math::InvSqrt(float fValue)
{
	return float(1.0f / sqrt(fValue));
}
//-----------------------------------------------------------------------
float Math::UnitRandom ()
{
	return float(rand()) / RAND_MAX;
}

//-----------------------------------------------------------------------
float Math::RangeRandom (float fLow, float fHigh)
{
	return (fHigh-fLow)*UnitRandom() + fLow;
}

//-----------------------------------------------------------------------
float Math::SymmetricRandom ()
{
	return 2.0f * UnitRandom() - 1.0f;
}

//-----------------------------------------------------------------------
void Math::setAngleUnit(Math::AngleUnit unit)
{
   msAngleUnit = unit;
}
//-----------------------------------------------------------------------
Math::AngleUnit Math::getAngleUnit(void)
{
   return msAngleUnit;
}
//-----------------------------------------------------------------------
float Math::AngleUnitsToRadians(float angleunits)
{
   if (msAngleUnit == AU_DEGREE)
	   return angleunits * fDeg2Rad;
   else
	   return angleunits;
}

//-----------------------------------------------------------------------
float Math::RadiansToAngleUnits(float radians)
{
   if (msAngleUnit == AU_DEGREE)
	   return radians * fRad2Deg;
   else
	   return radians;
}

//-----------------------------------------------------------------------
float Math::AngleUnitsToDegrees(float angleunits)
{
   if (msAngleUnit == AU_RADIAN)
	   return angleunits * fRad2Deg;
   else
	   return angleunits;
}

//-----------------------------------------------------------------------
float Math::DegreesToAngleUnits(float degrees)
{
   if (msAngleUnit == AU_RADIAN)
	   return degrees * fDeg2Rad;
   else
	   return degrees;
}

//-----------------------------------------------------------------------
bool Math::pointInTri2D(const Vector2D& p, const Vector2D& a,
	const Vector2D& b, const Vector2D& c)
{
	// Winding must be consistent from all edges for point to be inside
	Vector2D v1, v2;
	float dot[3];
	bool zeroDot[3];

	v1 = b - a;
	v2 = p - a;

	// Note we don't care about normalisation here since sign is all we need
	// It means we don't have to worry about magnitude of cross products either
	dot[0] = v1.crossProduct(v2);
	zeroDot[0] = Math::RealEqual(dot[0], 0.0f, 1e-3f);


	v1 = c - b;
	v2 = p - b;

	dot[1] = v1.crossProduct(v2);
	zeroDot[1] = Math::RealEqual(dot[1], 0.0f, 1e-3f);

	// Compare signs (ignore colinear / coincident points)
	if(!zeroDot[0] && !zeroDot[1]
	&& Math::Sign(dot[0]) != Math::Sign(dot[1]))
	{
		return false;
	}

	v1 = a - c;
	v2 = p - c;

	dot[2] = v1.crossProduct(v2);
	zeroDot[2] = Math::RealEqual(dot[2], 0.0f, 1e-3f);
	// Compare signs (ignore colinear / coincident points)
	if((!zeroDot[0] && !zeroDot[2]
		&& Math::Sign(dot[0]) != Math::Sign(dot[2])) ||
		(!zeroDot[1] && !zeroDot[2]
		&& Math::Sign(dot[1]) != Math::Sign(dot[2])))
	{
		return false;
	}


	return true;
}
//-----------------------------------------------------------------------
bool Math::pointInTri3D(const Vector3D& p, const Vector3D& a,
	const Vector3D& b, const Vector3D& c, const Vector3D& normal)
{
	// Winding must be consistent from all edges for point to be inside
	Vector3D v1, v2;
	float dot[3];
	bool zeroDot[3];

	v1 = b - a;
	v2 = p - a;

	// Note we don't care about normalisation here since sign is all we need
	// It means we don't have to worry about magnitude of cross products either
	dot[0] = v1.crossProduct(v2).dotProduct(normal);
	zeroDot[0] = Math::RealEqual(dot[0], 0.0f, 1e-3f);


	v1 = c - b;
	v2 = p - b;

	dot[1] = v1.crossProduct(v2).dotProduct(normal);
	zeroDot[1] = Math::RealEqual(dot[1], 0.0f, 1e-3f);

	// Compare signs (ignore colinear / coincident points)
	if(!zeroDot[0] && !zeroDot[1]
		&& Math::Sign(dot[0]) != Math::Sign(dot[1]))
	{
		return false;
	}

	v1 = a - c;
	v2 = p - c;

	dot[2] = v1.crossProduct(v2).dotProduct(normal);
	zeroDot[2] = Math::RealEqual(dot[2], 0.0f, 1e-3f);
	// Compare signs (ignore colinear / coincident points)
	if((!zeroDot[0] && !zeroDot[2]
		&& Math::Sign(dot[0]) != Math::Sign(dot[2])) ||
		(!zeroDot[1] && !zeroDot[2]
		&& Math::Sign(dot[1]) != Math::Sign(dot[2])))
	{
		return false;
	}


	return true;
}
//-----------------------------------------------------------------------
bool Math::RealEqual( float a, float b, float tolerance )
{
	if (fabs(b-a) <= tolerance)
		return true;
	else
		return false;
}

//-----------------------------------------------------------------------
std::pair<bool, float> Math::intersects(const Ray& ray, const Plane& plane)
{

	float denom = plane.normal.dotProduct(ray.getDirection());
	if (Math::Abs(denom) < std::numeric_limits<float>::epsilon())
	{
		// Parallel
		return std::pair<bool, float>(false, 0);
	}
	else
	{
		float nom = plane.normal.dotProduct(ray.getOrigin()) + plane.d;
		float t = -(nom/denom);
		return std::pair<bool, float>(t >= 0, t);
	}

}
//-----------------------------------------------------------------------
std::pair<bool, float> Math::intersects(const Ray& ray,
	const std::vector<Plane>& planes, bool normalIsOutside)
{
	std::vector<Plane>::const_iterator planeit, planeitend;
	planeitend = planes.end();
	bool allInside = true;
	std::pair<bool, float> ret;
	ret.first = false;
	ret.second = 0.0f;

	// derive side
	// NB we don't pass directly since that would require Plane::Side in
	// interface, which results in recursive includes since Math is so fundamental
	Plane::Side outside = normalIsOutside ? Plane::POSITIVE_SIDE : Plane::NEGATIVE_SIDE;

	for (planeit = planes.begin(); planeit != planeitend; ++planeit)
	{
		const Plane& plane = *planeit;
		// is origin outside?
		if (plane.getSide(ray.getOrigin()) == outside)
		{
			allInside = false;
			// Test single plane
			std::pair<bool, float> planeRes =
				ray.intersects(plane);
			if (planeRes.first)
			{
				// Ok, we intersected
				ret.first = true;
				// Use the most distant result since convex volume
#ifdef _MSC_VER
				ret.second = max(ret.second, planeRes.second);
#else
				ret.second = std::max(ret.second, planeRes.second);
#endif
			}
		}
	}

	if (allInside)
	{
		// Intersecting at 0 distance since inside the volume!
		ret.first = true;
		ret.second = 0.0f;
	}

	return ret;
}
//-----------------------------------------------------------------------
std::pair<bool, float> Math::intersects(const Ray& ray,
	const std::list<Plane>& planes, bool normalIsOutside)
{
	std::list<Plane>::const_iterator planeit, planeitend;
	planeitend = planes.end();
	bool allInside = true;
	std::pair<bool, float> ret;
	ret.first = false;
	ret.second = 0.0f;

	// derive side
	// NB we don't pass directly since that would require Plane::Side in
	// interface, which results in recursive includes since Math is so fundamental
	Plane::Side outside = normalIsOutside ? Plane::POSITIVE_SIDE : Plane::NEGATIVE_SIDE;

	for (planeit = planes.begin(); planeit != planeitend; ++planeit)
	{
		const Plane& plane = *planeit;
		// is origin outside?
		if (plane.getSide(ray.getOrigin()) == outside)
		{
			allInside = false;
			// Test single plane
			std::pair<bool, float> planeRes =
				ray.intersects(plane);
			if (planeRes.first)
			{
				// Ok, we intersected
				ret.first = true;
				// Use the most distant result since convex volume
#ifdef _MSC_VER
				ret.second = max(ret.second, planeRes.second);
#else
				ret.second = std::max(ret.second, planeRes.second);
#endif
			}
		}
	}

	if (allInside)
	{
		// Intersecting at 0 distance since inside the volume!
		ret.first = true;
		ret.second = 0.0f;
	}

	return ret;
}
//-----------------------------------------------------------------------
std::pair<bool, float> Math::intersects(const Ray& ray, const Sphere& sphere,
	bool discardInside)
{
	const Vector3D& raydir = ray.getDirection();
	// Adjust ray origin relative to sphere center
	const Vector3D& rayorig = ray.getOrigin() - sphere.getCenter();
	float radius = sphere.getRadius();

	// Check origin inside first
	if (rayorig.squaredLength() <= radius*radius && discardInside)
	{
		return std::pair<bool, float>(true, 0);
	}

	// Mmm, quadratics
	// Build coeffs which can be used with std quadratic solver
	// ie t = (-b +/- sqrt(b*b + 4ac)) / 2a
	float a = raydir.dotProduct(raydir);
	float b = 2 * rayorig.dotProduct(raydir);
	float c = rayorig.dotProduct(rayorig) - radius*radius;

	// Calc determinant
	float d = (b*b) - (4 * a * c);
	if (d < 0)
	{
		// No intersection
		return std::pair<bool, float>(false, 0);
	}
	else
	{
		// BTW, if d=0 there is one intersection, if d > 0 there are 2
		// But we only want the closest one, so that's ok, just use the
		// '-' version of the solver
		float t = ( -b - Math::Sqrt(d) ) / (2 * a);
		if (t < 0)
			t = ( -b + Math::Sqrt(d) ) / (2 * a);
		return std::pair<bool, float>(true, t);
	}


}
//-----------------------------------------------------------------------
std::pair<bool, float> Math::intersects(const Ray& ray, const AxisAlignedBox& box)
{
	if (box.isNull()) return std::pair<bool, float>(false, 0);
	if (box.isInfinite()) return std::pair<bool, float>(true, 0);

	float lowt = 0.0f;
	float t;
	bool hit = false;
	Vector3D hitpoint;
	const Vector3D& min = box.getMinimum();
	const Vector3D& max = box.getMaximum();
	const Vector3D& rayorig = ray.getOrigin();
	const Vector3D& raydir = ray.getDirection();

	// Check origin inside first
	if ( rayorig > min && rayorig < max )
	{
		return std::pair<bool, float>(true, 0);
	}

	// Check each face in turn, only check closest 3
	// Min x
	if (rayorig.x <= min.x && raydir.x > 0)
	{
		t = (min.x - rayorig.x) / raydir.x;
		if (t >= 0)
		{
			// Substitute t back into ray and check bounds and dist
			hitpoint = rayorig + raydir * t;
			if (hitpoint.y >= min.y && hitpoint.y <= max.y &&
				hitpoint.z >= min.z && hitpoint.z <= max.z &&
				(!hit || t < lowt))
			{
				hit = true;
				lowt = t;
			}
		}
	}
	// Max x
	if (rayorig.x >= max.x && raydir.x < 0)
	{
		t = (max.x - rayorig.x) / raydir.x;
		if (t >= 0)
		{
			// Substitute t back into ray and check bounds and dist
			hitpoint = rayorig + raydir * t;
			if (hitpoint.y >= min.y && hitpoint.y <= max.y &&
				hitpoint.z >= min.z && hitpoint.z <= max.z &&
				(!hit || t < lowt))
			{
				hit = true;
				lowt = t;
			}
		}
	}
	// Min y
	if (rayorig.y <= min.y && raydir.y > 0)
	{
		t = (min.y - rayorig.y) / raydir.y;
		if (t >= 0)
		{
			// Substitute t back into ray and check bounds and dist
			hitpoint = rayorig + raydir * t;
			if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
				hitpoint.z >= min.z && hitpoint.z <= max.z &&
				(!hit || t < lowt))
			{
				hit = true;
				lowt = t;
			}
		}
	}
	// Max y
	if (rayorig.y >= max.y && raydir.y < 0)
	{
		t = (max.y - rayorig.y) / raydir.y;
		if (t >= 0)
		{
			// Substitute t back into ray and check bounds and dist
			hitpoint = rayorig + raydir * t;
			if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
				hitpoint.z >= min.z && hitpoint.z <= max.z &&
				(!hit || t < lowt))
			{
				hit = true;
				lowt = t;
			}
		}
	}
	// Min z
	if (rayorig.z <= min.z && raydir.z > 0)
	{
		t = (min.z - rayorig.z) / raydir.z;
		if (t >= 0)
		{
			// Substitute t back into ray and check bounds and dist
			hitpoint = rayorig + raydir * t;
			if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
				hitpoint.y >= min.y && hitpoint.y <= max.y &&
				(!hit || t < lowt))
			{
				hit = true;
				lowt = t;
			}
		}
	}
	// Max z
	if (rayorig.z >= max.z && raydir.z < 0)
	{
		t = (max.z - rayorig.z) / raydir.z;
		if (t >= 0)
		{
			// Substitute t back into ray and check bounds and dist
			hitpoint = rayorig + raydir * t;
			if (hitpoint.x >= min.x && hitpoint.x <= max.x &&
				hitpoint.y >= min.y && hitpoint.y <= max.y &&
				(!hit || t < lowt))
			{
				hit = true;
				lowt = t;
			}
		}
	}

	return std::pair<bool, float>(hit, lowt);

}
//-----------------------------------------------------------------------
bool Math::intersects(const Ray& ray, const AxisAlignedBox& box,
	float* d1, float* d2)
{
	if (box.isNull())
		return false;

	if (box.isInfinite())
	{
		if (d1) *d1 = 0;
		if (d2) *d2 = Math::POS_INFINITY;
		return true;
	}

	const Vector3D& min = box.getMinimum();
	const Vector3D& max = box.getMaximum();
	const Vector3D& rayorig = ray.getOrigin();
	const Vector3D& raydir = ray.getDirection();

	Vector3D absDir;
	absDir[0] = Math::Abs(raydir[0]);
	absDir[1] = Math::Abs(raydir[1]);
	absDir[2] = Math::Abs(raydir[2]);

	// Sort the axis, ensure check minimise floating error axis first
	int imax = 0, imid = 1, imin = 2;
	if (absDir[0] < absDir[2])
	{
		imax = 2;
		imin = 0;
	}
	if (absDir[1] < absDir[imin])
	{
		imid = imin;
		imin = 1;
	}
	else if (absDir[1] > absDir[imax])
	{
		imid = imax;
		imax = 1;
	}

	float start = 0, end = Math::POS_INFINITY;

#define _CALC_AXIS(i)                                       \
do {                                                    \
	float denom = 1 / raydir[i];                         \
	float newstart = (min[i] - rayorig[i]) * denom;      \
	float newend = (max[i] - rayorig[i]) * denom;        \
	if (newstart > newend) std::swap(newstart, newend); \
	if (newstart > end || newend < start) return false; \
	if (newstart > start) start = newstart;             \
	if (newend < end) end = newend;                     \
} while(0)

	// Check each axis in turn

	_CALC_AXIS(imax);

	if (absDir[imid] < std::numeric_limits<float>::epsilon())
	{
		// Parallel with middle and minimise axis, check bounds only
		if (rayorig[imid] < min[imid] || rayorig[imid] > max[imid] ||
			rayorig[imin] < min[imin] || rayorig[imin] > max[imin])
			return false;
	}
	else
	{
		_CALC_AXIS(imid);

		if (absDir[imin] < std::numeric_limits<float>::epsilon())
		{
			// Parallel with minimise axis, check bounds only
			if (rayorig[imin] < min[imin] || rayorig[imin] > max[imin])
				return false;
		}
		else
		{
			_CALC_AXIS(imin);
		}
	}
#undef _CALC_AXIS

	if (d1) *d1 = start;
	if (d2) *d2 = end;

	return true;
}
//-----------------------------------------------------------------------
std::pair<bool, float> Math::intersects(const Ray& ray, const Vector3D& a,
	const Vector3D& b, const Vector3D& c, const Vector3D& normal,
	bool positiveSide, bool negativeSide)
{
	//
	// Calculate intersection with plane.
	//
	float t;
	{
		float denom = normal.dotProduct(ray.getDirection());

		// Check intersect side
		if (denom > + std::numeric_limits<float>::epsilon())
		{
			if (!negativeSide)
				return std::pair<bool, float>(false, 0);
		}
		else if (denom < - std::numeric_limits<float>::epsilon())
		{
			if (!positiveSide)
				return std::pair<bool, float>(false, 0);
		}
		else
		{
			// Parallel or triangle area is close to zero when
			// the plane normal not normalised.
			return std::pair<bool, float>(false, 0);
		}

		t = normal.dotProduct(a - ray.getOrigin()) / denom;

		if (t < 0)
		{
			// Intersection is behind origin
			return std::pair<bool, float>(false, 0);
		}
	}

	//
	// Calculate the largest area projection plane in X, Y or Z.
	//
	size_t i0, i1;
	{
		float n0 = Math::Abs(normal[0]);
		float n1 = Math::Abs(normal[1]);
		float n2 = Math::Abs(normal[2]);

		i0 = 1; i1 = 2;
		if (n1 > n2)
		{
			if (n1 > n0) i0 = 0;
		}
		else
		{
			if (n2 > n0) i1 = 0;
		}
	}

	//
	// Check the intersection point is inside the triangle.
	//
	{
		float u1 = b[i0] - a[i0];
		float v1 = b[i1] - a[i1];
		float u2 = c[i0] - a[i0];
		float v2 = c[i1] - a[i1];
		float u0 = t * ray.getDirection()[i0] + ray.getOrigin()[i0] - a[i0];
		float v0 = t * ray.getDirection()[i1] + ray.getOrigin()[i1] - a[i1];

		float alpha = u0 * v2 - u2 * v0;
		float beta  = u1 * v0 - u0 * v1;
		float area  = u1 * v2 - u2 * v1;

		// epsilon to avoid float precision error
		const float EPSILON = 1e-3f;

		float tolerance = - EPSILON * area;

		if (area > 0)
		{
			if (alpha < tolerance || beta < tolerance || alpha+beta > area-tolerance)
				return std::pair<bool, float>(false, 0);
		}
		else
		{
			if (alpha > tolerance || beta > tolerance || alpha+beta < area-tolerance)
				return std::pair<bool, float>(false, 0);
		}
	}

	return std::pair<bool, float>(true, t);
}
//-----------------------------------------------------------------------
std::pair<bool, float> Math::intersects(const Ray& ray, const Vector3D& a,
	const Vector3D& b, const Vector3D& c,
	bool positiveSide, bool negativeSide)
{
	Vector3D normal = calculateBasicFaceNormalWithoutNormalize(a, b, c);
	return intersects(ray, a, b, c, normal, positiveSide, negativeSide);
}
//-----------------------------------------------------------------------
bool Math::intersects(const Sphere& sphere, const AxisAlignedBox& box)
{
	if (box.isNull()) return false;
	if (box.isInfinite()) return true;

	// Use splitting planes
	const Vector3D& center = sphere.getCenter();
	float radius = sphere.getRadius();
	const Vector3D& min = box.getMinimum();
	const Vector3D& max = box.getMaximum();

	// Arvo's algorithm
	float s, d = 0;
	for (int i = 0; i < 3; ++i)
	{
		if (center.ptr()[i] < min.ptr()[i])
		{
			s = center.ptr()[i] - min.ptr()[i];
			d += s * s;
		}
		else if(center.ptr()[i] > max.ptr()[i])
		{
			s = center.ptr()[i] - max.ptr()[i];
			d += s * s;
		}
	}
	return d <= radius * radius;

}
//-----------------------------------------------------------------------
bool Math::intersects(const Plane& plane, const AxisAlignedBox& box)
{
	return (plane.getSide(box) == Plane::BOTH_SIDE);
}
//-----------------------------------------------------------------------
bool Math::intersects(const Sphere& sphere, const Plane& plane)
{
	return (
		Math::Abs(plane.getDistance(sphere.getCenter()))
		<= sphere.getRadius() );
}
//-----------------------------------------------------------------------
Vector3D Math::calculateTangentSpaceVector(
	const Vector3D& position1, const Vector3D& position2, const Vector3D& position3,
	float u1, float v1, float u2, float v2, float u3, float v3)
{
	//side0 is the vector along one side of the triangle of vertices passed in,
	//and side1 is the vector along another side. Taking the cross product of these returns the normal.
	Vector3D side0 = position1 - position2;
	Vector3D side1 = position3 - position1;
	//Calculate face normal
	Vector3D normal = side1.crossProduct(side0).normalize();
	//Now we use a formula to calculate the tangent.
	float deltaV0 = v1 - v2;
	float deltaV1 = v3 - v1;
	Vector3D tangent = deltaV1 * side0 - deltaV0 * side1;
	tangent.normalize();
	//Calculate binormal
	float deltaU0 = u1 - u2;
	float deltaU1 = u3 - u1;
	Vector3D binormal = deltaU1 * side0 - deltaU0 * side1;
	binormal.normalize();
	//Now, we take the cross product of the tangents to get a vector which
	//should point in the same direction as our normal calculated above.
	//If it points in the opposite direction (the dot product between the normals is less than zero),
	//then we need to reverse the s and t tangents.
	//This is because the triangle has been mirrored when going from tangent space to object space.
	//reverse tangents if necessary
	Vector3D tangentCross = tangent.crossProduct(binormal);
	if (tangentCross.dotProduct(normal) < 0.0f)
	{
		tangent = -tangent;
		binormal = -binormal;
	}

	return tangent;

}
//-----------------------------------------------------------------------
Matrix4 Math::buildReflectionMatrix(const Plane& p)
{
	return Matrix4(
		-2 * p.normal.x * p.normal.x + 1,   -2 * p.normal.x * p.normal.y,       -2 * p.normal.x * p.normal.z,       -2 * p.normal.x * p.d,
		-2 * p.normal.y * p.normal.x,       -2 * p.normal.y * p.normal.y + 1,   -2 * p.normal.y * p.normal.z,       -2 * p.normal.y * p.d,
		-2 * p.normal.z * p.normal.x,       -2 * p.normal.z * p.normal.y,       -2 * p.normal.z * p.normal.z + 1,   -2 * p.normal.z * p.d,
		0,                                  0,                                  0,                                  1);
}
//-----------------------------------------------------------------------
Vector4D Math::calculateFaceNormal(const Vector3D& v1, const Vector3D& v2, const Vector3D& v3)
{
	Vector3D normal = calculateBasicFaceNormal(v1, v2, v3);
	// Now set up the w (distance of tri from origin
	return Vector4D(normal.x, normal.y, normal.z, -(normal.dotProduct(v1)));
}
//-----------------------------------------------------------------------
Vector3D Math::calculateBasicFaceNormal(const Vector3D& v1, const Vector3D& v2, const Vector3D& v3)
{
	Vector3D normal = (v2 - v1).crossProduct(v3 - v1);
	normal.normalize();
	return normal;
}
//-----------------------------------------------------------------------
Vector4D Math::calculateFaceNormalWithoutNormalize(const Vector3D& v1, const Vector3D& v2, const Vector3D& v3)
{
	Vector3D normal = calculateBasicFaceNormalWithoutNormalize(v1, v2, v3);
	// Now set up the w (distance of tri from origin)
	return Vector4D(normal.x, normal.y, normal.z, -(normal.dotProduct(v1)));
}
//-----------------------------------------------------------------------
Vector3D Math::calculateBasicFaceNormalWithoutNormalize(const Vector3D& v1, const Vector3D& v2, const Vector3D& v3)
{
	Vector3D normal = (v2 - v1).crossProduct(v3 - v1);
	return normal;
}
//-----------------------------------------------------------------------
float Math::gaussianDistribution(float x, float offset, float scale)
{
	float nom = Math::Exp(
		-Math::Sqr(x - offset) / (2 * Math::Sqr(scale)));
	float denom = scale * Math::Sqrt(2 * Math::PI);

	return nom / denom;

}

float Math::frand()
{
	return rand()/(float)RAND_MAX;
}

float Math::randfloat(float lower, float upper)
{
	return lower + (upper-lower)*(rand()/(float)RAND_MAX);
}

int Math::randint(int lower, int upper)
{
	return lower + (int)((upper+1-lower)*frand());
}

bool Math::isZero(Vector3D& v)
{
	if(v.length() <= std::numeric_limits<float>::epsilon())
		return true;

	return false;
}

float Math::getDegreeBetween(Vector3D& v1, Vector3D& v2)
{
	float lenProduct = v1.length() * v2.length();

	// Divide by zero check
	if(lenProduct < std::numeric_limits<float>::epsilon())
		lenProduct = std::numeric_limits<float>::epsilon();

	float f = v1.dotProduct(v2) / lenProduct;

	float deg = acos(f) * 180.0f / Math::PI;

	return deg;

}

float Math::getRadianBetween(Vector3D& v1, Vector3D& v2)
{
	float lenProduct = v1.length() * v2.length();

	// Divide by zero check
	if(lenProduct < std::numeric_limits<float>::epsilon())
		lenProduct = std::numeric_limits<float>::epsilon();

	float f = v1.dotProduct(v2) / lenProduct;

	return acos(f);
}
